/*** Servo module   grytole@gmail.com   03-Nov-2013 ***/

#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include "servo.h"

#define SERVO_REPEAT_POS (40000)
#define SERVO_DDR (DDRB)
#define SERVO_PORT (PORTB)
#define SERVO_1 (PB5)
#define SERVO_2 (PB6)
#define SERVO_3 (PB7)
#define SERVO_PINS ((1 << SERVO_1) | (1 << SERVO_2) | (1 << SERVO_3))

static volatile uint16_t m_servoPos[SERVO_NUM];
static volatile uint8_t  m_servoMap[SERVO_NUM];
static volatile uint8_t  m_servoCur;

/*** Servo control signal end handler ***/
ISR(TIMER1_COMPA_vect)
{
    SERVO_PORT &= ~(1 << m_servoMap[m_servoCur]);            /* set current servo pin as low */
}

/*** Servo frame end handler ***/
ISR(TIMER1_COMPB_vect)
{
    m_servoCur += 1;                                         /* find next servo id */
    if(m_servoCur == SERVO_NUM)
    {
        m_servoCur = 0;
    }

    SERVO_PORT |= (1 << m_servoMap[m_servoCur]);             /* set next servo pin as high */

    OCR1A = m_servoPos[m_servoCur];                          /* load next servo position */
    TCNT1 = 0;                                               /* reset timer */
}

/*** Module initialization ***/
void servoInit(void)
{
    SERVO_DDR |= SERVO_PINS;                                 /* set all servo pins as outputs */
    SERVO_PORT &= ~(SERVO_PINS);                             /* set all servo pins as low */

    m_servoPos[0] = SERVO_DEFAULT_POS;
    m_servoPos[1] = SERVO_DEFAULT_POS;
    m_servoPos[2] = SERVO_DEFAULT_POS;
    m_servoMap[0] = SERVO_1;
    m_servoMap[1] = SERVO_2;
    m_servoMap[2] = SERVO_3;
    m_servoCur = 0;

    OCR1A = m_servoPos[m_servoCur];                          /* load current servo position */
    OCR1B = SERVO_REPEAT_POS;                                /* set frame size for servo */
    TCNT1 = 0;                                               /* reset timer */
    TIMSK |= (1 << OCIE1A) | (1 << OCIE1B);                  /* enable compare match interrupts */
    TCCR1B = (1 << CS10);                                    /* enable clock source for timer without prescaler */
    SERVO_PORT |= (1 << m_servoMap[m_servoCur]);             /* set current servo pin as high */
}

/*** Setting of new servo position values ***/
void servoSetPosition(uint8_t servoId, int16_t servoPos)
{
    if(SERVO_NUM > servoId)                                  /* use only available servos */
    {
        if((SERVO_START_POS - SERVO_DEFAULT_POS) > servoPos) /* position should be in available limits */
        {
            servoPos = SERVO_START_POS - SERVO_DEFAULT_POS;
        }

        if((SERVO_STOP_POS - SERVO_DEFAULT_POS) < servoPos)  /* position should be in available limits */
        {
            servoPos = SERVO_STOP_POS - SERVO_DEFAULT_POS;
        }

        m_servoPos[servoId] = SERVO_DEFAULT_POS + servoPos;  /* set new servo position */
    }
}

/*** Get pointer on servo position storage ***/
uint8_t* servoGetDataPtr(uint8_t servoId)
{
    uint8_t* dataPtr = 0;

    if(SERVO_NUM > servoId)                                  /* use only available servos */
    {
        dataPtr = (uint8_t*)(&m_servoPos[servoId]);
    }

    return dataPtr;
}

/*** eof ***/